#include "./device_info.h"
#include "../driver/spi_flash.h"
#include "../driver/robot_arm1.h"
#include "../driver/robot_arm2.h"

namespace service {

static constexpr uint32_t MaxCargoCount = 50;
namespace ec = utils::ec;
using namespace driver;
struct cargo_infos_t {
    
    utils::cargo_t infos[MaxCargoCount];
    uint16_t pick_p1;
    uint16_t pick_p2;
    uint16_t pick_depth;

    void reset() {
        for (uint32_t i = 0; i < MaxCargoCount; i ++) {
            infos[i].invalid();
        }
        pick_p1 = RobotArm1::ORIGIN_POSITION;
        pick_p2 = RobotArm2::ORIGIN_POSITION;
        pick_depth = 600;
    }
};

static driver::store_t<driver::DeviceInfoAddr, cargo_infos_t> info;

DeviceInfo::DeviceInfo() {
    info.load();
}

void DeviceInfo::set_pick_info(uint16_t depth) {
    info->pick_p1 = robotArm1->get_position();
    info->pick_p2 = robotArm2->get_position();
    info->pick_depth = depth;
    info.save();
}

void DeviceInfo::set_pick_info(uint16_t p1, uint16_t p2, uint16_t depth) {
    info->pick_p1 = p1;
    info->pick_p2 = p2;
    info->pick_depth = depth;
    info.save();
}

void DeviceInfo::get_pick_info(uint16_t &p1, uint16_t &p2, uint16_t &depth) {
    p1 = info->pick_p1;
    p2 = info->pick_p2;
    depth = info->pick_depth;
}

utils::error_t DeviceInfo::get_by_cargo(utils::cargo_t &cargo) {
    for (uint32_t i = 0; i < MaxCargoCount; i ++) {
        auto &c = info->infos[i];
        if (c == cargo) {
            cargo = c;
            return ec::Ok;
        } 
    }
    return ec::CargoNotFind;
}

uint32_t DeviceInfo::get_cargo_count() {
    uint32_t count = 0;
    for (uint32_t i = 0; i < MaxCargoCount; i ++) {
        if (info->infos[i].is_valid()) {
            count ++;
        }
    }
    return count;
}

utils::error_t DeviceInfo::change_or_add_cargo(const utils::cargo_t &cargo) {
    for (uint32_t i = 0; i < MaxCargoCount; i ++) {
        auto &c = info->infos[i];
        if (c == cargo) {
            c = cargo;
            info.save();
            return ec::Ok;
        } 
    } 
    for (uint32_t i = 0; i < MaxCargoCount; i ++) {
        auto &c = info->infos[i];
        if (!c.is_valid()) {
            c = cargo;
            info.save();
            return ec::Ok;
        } 
    } 

    return ec::CargoFull;
}

void DeviceInfo::for_each(void (*f) (const utils::cargo_t &)) {
    for (uint32_t i = 0; i < MaxCargoCount; i ++) {
        auto &c = info->infos[i];
        if (c.is_valid()) {
            f(c);
        }
    }
}

void DeviceInfo::notify_info(const utils::message_t &msg) {
    for (uint32_t i = 0; i < MaxCargoCount; i ++) {
        auto &c = info->infos[i];
        if (!c.is_valid()) {
            continue;
        }
        msg.write(utils::proto::QueryAllCargo | utils::proto::NotifyFlag,
            c.row,
            c.col,
            c.p1,
            c.p2,
            c.depth1,
            c.depth2
        );
    }
}

utils::error_t DeviceInfo::del_cargo(const utils::cargo_t &cargo) {
    for (uint32_t i = 0; i < MaxCargoCount; i ++) {
        auto &c = info->infos[i];
        if (c == cargo) {
            c.invalid();
            info.save();
            return ec::Ok;
        }
    }
    return ec::CargoNotFind;
}


}
